#ifndef CPUREPURSUIT_H
#define CPUREPURSUIT_H

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>

#include "matplotlibcpp.h"

using namespace std;
namespace plt = matplotlibcpp;

typedef struct {
    int idxTarget;
    vector<double> lookAheadPoint;
} look_ahead_t;

typedef struct {
    vector<double> pos;
    double heading;
    double v;
} new_state_t;

class CPurePursuit
{
public:
    vector<double> XRef;
    vector<double> YRef;
    vector<vector<double>> posRef;

    vector<vector<double>> posActual;
    vector<double> headingActual;
    vector<double> vActual;
    // 车辆初始位置
    vector<double> pos;
    double v;
    double heading;

    // 有关参数
    double Kv;
    double Kp;
    double Ld0;
    double dt;
    double L;
    double targetSpeed;

    int idxTarget;

    vector<double> XActual;
    vector<double> YActual;

public:
    CPurePursuit();
    ~CPurePursuit();

    inline double normFunc(double posRefX, double posRefY, double posX, double posY);
    //读取路径点
    vector<vector<double>> readPosPoints(vector<double> &XRef, vector<double> &YRef);

    look_ahead_t findLookAheadPoint(vector<double> pos, double v, vector<vector<double>> posRef, double Kv, double Ld0);

    double purePursuit(vector<double> lookAheadPoint, int idxTarget,
                       vector<double> pos, double heading, double v,
                       vector<vector<double>> posRef, double Kv, double Ld0, double L);

    new_state_t updataState(double a, vector<double> pos, double heading, double v, double delta, double L, double dt);

    void plotRes(vector<double> XRef, vector<double> YRef, vector<double> XActual, vector<double> YActual);
};

#endif // CPUREPURSUIT_H
